import time

from trajectories.traj_base import BaseTrajectory
from utils.b_spline_geomdl import approximate_b_spline


class BSplineCurveTrajectory(BaseTrajectory):
    TRACK_KEY = "BSplineCurve"

    def __init__(self):
        super().__init__()

    def get_track_key(self):
        return self.TRACK_KEY

    def get_color(self):
        return [81, 81, 231]

    def tracking(self, args, update_cb, ls):
        # start_point, end_point = ls.forward_ratio(np.array(args))

        s_arg, times_arg, way_points_lst = args
        # way_points_lst = ls.forward_ratio(np.array(way_points))

        self.path_angles.clear()

        for way_points in way_points_lst:
            n_course_point = len(way_points) * times_arg  # sampling number
            curve_points = approximate_b_spline(way_points, degree=s_arg, count=n_course_point)

            slice_count = len(curve_points)
            slice_time_second = (1 / slice_count) * 0.1
            # 绘制路径
            for p in curve_points:
                if ls.try_update_point_end(p):
                    alpha, beta = ls.inverse_not_update(p)
                    self._add_angles((alpha, beta))
                    self._add_point(p)
                    update_cb()
                    time.sleep(slice_time_second)

            self._add_angles((999, 999))
            self._add_point(ls.forward_ratio([999, 55]))



